#include "zuc/nml_intf/zucpos.h"
#include "zuc/nml_intf/zucpos2.h"
#include "zuc/nml_intf/zuc_nml_rob.hh"

// 坐标系、负载设置指令
extern int zucTrajSetPayload(int id, Payload payload);
extern int zucSetRobotPayload(int robid, int id);
extern int zucTrajSetRobotToolID(int group_id, int id);
extern int zucTrajSetRobotUserID(int robot_id, int id);
extern int zucTrajSetRobotToolOffset(ZucPose robotToolOffset, unsigned char method, int id, const char* name);
extern int zucTrajSetRobotToolOffset(ZucPose robotToolOffset);
extern int zucTrajSetRobotUserOffset(int robot_id, ZucPose robotUserFrame);
extern int zucTrajSetRobotUserOffset(ZucPose user_offset, unsigned char method, int id, const char* name);

// 状态切换指令
extern int zucRobotPowerOn(int robid);
extern int zucRobotPowerOff(int robid);
extern int zucTrajEnable(int robid);
extern int zucTrajDisable(int robid);
extern int boot_robot(int robid);
extern int zucSetDragEnable(int robid, int enable);
extern int zucSetServoEnable(int robid, int enable);
extern int zucServoHome(int robot_id, int joint);

// 运动指令 --- JOG
extern int zucJogStop(int robid);
extern int zucJogMultiJoint(int robid, double vel, RobJointVal pos);
extern int zucJogAbs(int robid, int nr, double pos, double vel, int jjogmode);
extern int zucJogCont(int robid, int nr, double vel, int jjogmode);
extern int zucJogIncr(int robid, int nr, double incr, double vel, int jjogmode);

// 对外状态更新
extern int zucTorqSensorUpdate(ZUC_SENSOR_STAT* stat);
extern int zucUpdate(ZUC_STAT* stat);                                            // 更新本控制柜的共有状态
extern int zucMotionUpdate(ZUC_MOTION_STAT* stat, ZUC_MOTION_STAT* multi_stat);  // 更新每个机器人的状态
extern int zucJointUpdate(int robid, ZUC_JOINT_STAT stat[]);
extern int zucMonitorUpdate(ZUC_MONITOR_STAT* stat);

// 机器人安全配置
extern int zucSetServoParam(int robot_id, ServoParam param);
extern int zucSetRobotSafety(ZUC_MOTION_STAT* stat, ZUC_ROBOT_SAFETY robot_safety);
extern int zucSetRobotTcpVelLimit(float tcpVelLimit);
extern int zucSetRobotMomentumLimit(float momentum);
extern int zucSetRobotStoppingDist(float stoppingDist);
extern int zucSetRobotStoppingTime(float stoppingTime);
extern int zucSetRobotElbowVelLimitReduce(double elbowVelLimitReduce);
extern int zucSetRobotTcpVelLimitReduce(double tcp_speed);
extern int zucSetRobotMomentumLimitReduce(double momentum_limit);
extern int zucSetRobotPowerLimitReduce(double power_limit);
extern int zucSetRobotStoppingDistReduce(double stopping_dict);
extern int zucSetRobotStoppingTimeReduce(double stopping_time);
extern int zucSetRobotPowerLimit(float robotPowerLimit, int saveFlag);
extern int updateRobotSafety(int model);
extern int updateJointSafety(int model, int serial);
extern int zucSetCustomizeType(int customize_type);
extern int zucSetReduceSafety(ZUC_MOTION_STAT* stat, reduce_param_config reduce_param);
extern int zucSetDragTcpSpeedLimitSetting(double dragTcpSpeedLimit);
extern int zucGetRobotSafety(ZUC_MOTION_STAT* stat);
extern int zucGetJointSafety(ZUC_MOTION_STAT* stat);
extern int zucSetDragTcpSpeedLimit(double dragTcpVelLimit);
extern int
zucSetAttitudeSafety(int choice, double tilt, double pan, double deviation, double deviation_warning, double cenrpy[3], int enable_open, int enable_auto);

extern int zucStartIdentifyFriction(int robot_id, int friction_identify_joint_index);
extern int zucStopIdentifyFriction(int robot_id, int friction_identify_joint_index);
extern int zucSetServoDynamicsPara(int robot_id, const int* servo_dynamics_para);
extern int zucGetRs485Message(int robot_id, int chn, int len);
extern int zucSetServoFriction(int robot_id, const double* friction_para, short jnt_id);
extern int zucSetNetExceptionHandle(float timeLimit, int motType);
extern int zucSetCabVoltage(unsigned int cabVoltage);
extern int zucSetPlannerJerk(double joint_jerk, double carte_jerk);
extern int zucSetOperationMode(unsigned int mode);
extern int zucSetOperationModeOption(unsigned int enable);

extern int zucTaskInitFinish();
extern int zucSyncZucsettings();
extern int SetRobot7dofinv_inform(int inv_flag, double arm_angle);
extern int SetRobot7dof_kine_param_alpha_k(double k, double alpha);
extern int SetRobotFull_DH_flag(int full_dh_flag);

extern int zucSetSafeZoneEnableOpt(int openEna, int autoRunEna);
extern int zucUpdateSafeZoneEna();
extern int zucSetSafeZone(SafeZone zone);
extern int zucSetSafeZoneElbowWristSphereOpt(double SafezoneElbowRadius, double SafezoneElbowOffest);
extern int zucUpdateSafeZoneElbowWristSphere();
extern int
zucSetFunctionSaftyPara(const double* robot_power_limit, const double* robot_momentum_limit, const double* robot_endforce_limit, double robot_velocity_limit);

extern int zucStartPayloadIdentifyTraj(int trajId, JointTraj jointTraj, int withPayload);
extern int zucResetPayloadIdentifyStatus(int userOpt);
extern int zucGetPayloadIdentifyResult();

extern int zucSetTorqueControlEnable(int torque_control_enable, int period);
extern int zucStartDynamicsIdentify();
extern int zucStopDynamicsIdentify();
extern int zucResetDynamicsIdentify();
extern int zucCompleteDynamicsIdentify();
extern int zucGetDynamicsIdentifyResult();

extern int zucSetFuncDIMap(FuncIOMap funcIOMap, int saveFlag);
extern int zucSetFuncDOMap(FuncIOMap funcIOMap, int saveFlag);
extern int zucSetSafetyFuncDIMap(SafetyFuncDIMap sFuncDIMap, int saveFlag);
extern int zucSetSafetyFuncDOMap(SafetyFuncDOMap sFuncDOMap, int saveFlag);
extern int zucSetCabinetId(unsigned int cabId, int saveFlag);

int zucSetTrajSmooth(int trqLimitActive, int firFltActive);

extern int zucCalibDHParams(DHParam dhParamError);

// implementation functions for ZUC_AXIS types
extern double zucAxisGetMaxVelocity(int axis);
extern double zucAxisGetMaxAcceleration(int axis);
extern double zucJointGetMaxVelocity(int joint);
extern double zucJointGetMaxAcceleration(int joint);
extern double zucTrajGetLinearUnits();
extern double zucTrajGetAngularUnits();

// implementation functions for ZUC_JOINT types

extern int zucJointLoadComp(int joint, const char* file, int type);

extern int zucJogMovc(int mvOpt,
                      double* pos_mid,
                      double* pos_end,
                      double vel,
                      double acc,
                      double tol,
                      int circle_cnt,
                      int executing_line_id,
                      int* end_cond,
                      int circlemode);

// implementation functions for ZUC_TRAJ types
extern int zucTrajSetCycleTime(double cycleTime);
extern int zucTrajSetMode(int traj_mode);
extern int zucTrajSetScale(double scale);
extern int zucTrajSetRapidScale(double scale, int robot_id = -1);
extern int zucTrajSetFOEnable(unsigned char mode);    //feed override enable
extern int zucTrajSetSOEnable(unsigned char mode);    //spindle speed override enable
extern int zucTrajSetAFEnable(unsigned char enable);  //adaptive feed enable
extern int zucTrajSetMotionId(int id);
extern int zucTrajSetAdmittanceOption(int* option);
extern int zucTrajSetEndForceCond(int axis, EndForceCond endForceCond);
extern int zucTrajSetAdmittanceEnable(int enable);
extern int zucTrajDisableForceControl(int disableForceCtrl);
extern int zucTrajGetUseTopp(int use_topp);
extern int zucTrajGetFilterSize(int filter_size);
extern int zucTrajGetAddTorCons(int add_tor_cons);
extern int zucTrajGetTorPortion(double tor_portion);
extern int zucTrajSetDisJLen(double dislen);
extern int zucTrajSetDisLLen(double dislen);
extern int zucTrajSetDisCLen(double dislen);
extern int zucTrajSetSmoothPara(double smooth_para);
extern int zucTrajSetRateBlendPara(int rate_blend, int topp_queue_len, int max_fw_len);
extern double zucTrajGetMaxAcceleration(void);
extern double zucTrajGetMaxVel(void);
extern double zucTrajGetMaxJointJerk(void);
extern double zucTrajGetMaxCarteJerk(void);
extern int zucTrajAbort();
extern int zucTrajPause(int robot_id = -1);
extern int zucTrajStep();
extern int zucTrajResume(int robot_id = -1);
extern int zucTrajDelay(double delay);
extern int zucTrajLinearMove(ZucPose end,
                             int type,
                             double vel,
                             double ini_maxvel,
                             double acc,
                             double jerk,
                             double ori_vel,
                             double ori_acc,
                             int indexrotary,
                             int using_abc,
                             int di_type,
                             int di_index,
                             int di_state,
                             int planner_type);
extern int
zucTrajCircularMove(ZucPose end, PM_CARTESIAN center, PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc, double jerk);
extern int zucTrajJointMove(ZucPose end,
                            int type,
                            double velPercent,
                            double ini_maxvel,
                            double acc,
                            double jerk,
                            int indexrotary,
                            int di_type,
                            int di_index,
                            int di_state,
                            int jointNum,
                            int planner_type);
extern int zucTrajMovs(const double* a0, const double* a1, const double* a2, const double* a3, double time);
extern int zucTrajMovc(ZucPose mid,
                       ZucPose end,
                       double vel,
                       double ini_maxvel,
                       double acc,
                       double jerk,
                       int using_abc,
                       double circle_count,
                       int di_type,
                       int di_index,
                       int di_state,
                       int planner_type,
                       int circlemode);
extern int zucTrajSetTermCond(int cond, double tolerance);
extern int zucTrajSetSafeAttitude(PmRpy conicalRpy,
                                  double tilt,
                                  double pan,
                                  double deviation,
                                  double deviationwarn,
                                  int openEnable,
                                  int autoEnable,
                                  int tipChoice,
                                  int saveOption);

extern int zucTrajConveyorLinearEnable(double x, double y, double z, double pulseEquivalent, int convyr_type, double max_distance);
extern int zucTrajConveyorCircularEnable(double p1x,
                                         double p1y,
                                         double p1z,
                                         double p2x,
                                         double p2y,
                                         double p2z,
                                         double p3x,
                                         double p3y,
                                         double p3z,
                                         double pulseEquivalent,
                                         int convyr_type,
                                         int rotate_tool,
                                         double max_distance);
extern int zucTrajConveyorDisable(void);
extern int zucTrajSetOrigin(ZucPose origin);
extern int zucTrajSetRotation(double rotation);
extern int zucTrajRigidTap(ZucPose pos, double vel, double ini_maxvel, double acc);
extern int zucTrajUpdate(int robid, ZUC_TRAJ_STAT* stat);
extern int zucTrajCalcTcp(const unsigned char id,
                          const unsigned char saveType,
                          const unsigned char statusFlag,
                          const double* jntPos1,
                          const double* jntPos2,
                          const double* jntPos3,
                          const double* jntPos4);
extern int
zucTrajCalcSafeAtt(const ZucPose* cartpos1, const ZucPose* cartpos2, const ZucPose* cartpos3, const double* tilt, const double* pan, const int* tipChoice);
// extern int zucTrajCalcTcp_six(const double* jntPos1, const double* jntPos2, const double* jntPos3, const double* jntPos4);
extern int zucTrajCalcTcp_six(const unsigned char id,
                              const unsigned char saveType,
                              const unsigned char statusFlag,
                              const double* jntPos1,
                              const double* jntPos2,
                              const double* jntPos3,
                              const double* jntPos4,
                              const double* jntPos5,
                              const double* jntPos6);
extern int zucTrajCalcUser(const unsigned char id,
                           const unsigned char saveType,
                           const unsigned char statusFlag,
                           const double* jntPos1,
                           const double* jntPos2,
                           const double* jntPos3);

extern int zucToppraFinishLeftTc(void);
extern int zucToppraFirstCommand(void);
extern int cal_using_abc(ZucPose start_cartpos, ZucPose end, double vel, double acc, double ori_vel, double ori_acc, int* using_abc, double* tmag_abc);
extern int zucTrackingConfig(int linkdi, int comcyclenum, double synchacc, int lctsynch);
extern int zucSetDHCom(int full_dh_com);
extern int zucSetLimitLinearVelocity(int enable);
extern int zucSetLimitJointVelocity(int enable);
extern int zucSetTcpMaxError(double max_error);
extern int zucSetPBblendMode(int pb_blend_mode);
extern int zucSetMovejMovelBlend(int enable);
extern int zucSetMovejMovejBlend(int enable);
extern int zucAdapt2Robot(unsigned int robotSerialNum);
extern int zucGetModelFromSerialNum(unsigned int serialNum, int* robotModel);
extern int zucSetCartJogAcc(double* step_jog_pos_acc, double* step_jog_ori_acc);

extern int zucExtIOUpdate(ZUC_EXTIO_STAT* stat);
extern int zucExtIOSetup(int extIOModCnt, ExtIOSetup* extIOSetups, int saveFlag);
extern int zucExtIOSetMode(int mode);
extern int zucExtIOSetReconnectParams(int reConnectInterval, int reConnectMaxTimes, int saveFlag);
extern int zucSetMaintenanceMode(int maintenance);
extern int zucModbusSetupSlave(int commType, CommInfo mbComm);

// torque sensor configuration

extern int zucTioMBDelSignal(const char* name);
extern int zucTioMBUpdateSignal(const char* name, float freq);
extern int zucTioMBSendCommand(int chnId, const unsigned char* cmd, int len);

// tool configuration

// implementation functions for ZUC_MOTION aggregate types
extern int zucMotionInit();
extern int zucMotionAbort(int robot_id = -1);
extern int zucMotionSetDebug(int debug);
extern int zucMotionSetRunMode(int robid, int mode, int serial_num);
extern int zucMotionSetAout(unsigned char ioType, unsigned char index, double start, double end, unsigned char now);
extern int zucMotionSetDout(unsigned char ioType, int index, unsigned char start, unsigned char end, unsigned char now);
extern int zucSetCollisionGmRecordEnable(int gm_record_enable);

//extern int zucMotionAPP(unsigned char appver);

extern int zucMotionExitProtect(int type);
extern int zucMotionPercentageMode(int level);
extern int zucMotionPercentageModeLevelValue(double percentage_mode_level1_val, double percentage_mode_level2_val);
extern int zucMotReduceMode(int mode);

extern int get_toppra_related_params(const int discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit);
extern void get_lasttc_blend_jpos(ZucJPose* blend_jpos_for_fit);
extern void update_related_params();
extern double get_vel_by_stopdisttime(double jerk, int motion_type, int planner_type);
extern void update_related_params2();  // update toppra related parameters when current command did not use toppra;
extern void get_last_pos();
extern int movj_get_toppra_params();  //获取用于求解可控集，可达集和后面更新位置所需要的参数
extern int movl_get_toppra_params(ZucPose start_cartpos,
                                  KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                                  KINEMATICS_INVERSE_FLAGS* toppra_iflags,
                                  double tmag_abc);  //获取用于求解可控集，可达集和后面更新位置所需要的参数
extern int movc_get_toppra_params();                 //获取用于求解可控集，可达集和后面更新位置所需要的参数
extern int get_pathvalue_coefs_pycubic(const int discrete_num, const ZucJPose* jpos_for_fit);  //获取拟合得到的系数和路径参数的值with python
extern int get_blend_pathvalue_coefs_csaps(const int discrete_num, const double start_path_data, const ZucJPose* jpos_for_fit);
extern int get_joint_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit);  //根据运动指令得到离散点
extern int get_movl_discrete_info(ZucPose start_cartpos,
                                  int& discrete_num,
                                  ZucJPose* jpos_for_fit,
                                  ZucJPose* blend_jpos_for_fit,
                                  KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                                  KINEMATICS_INVERSE_FLAGS* toppra_iflags);  //根据运动指令得到离散点
extern int get_movl_discrete_info_abc(ZucPose start_cartpos,
                                      int& discrete_num,
                                      ZucJPose* jpos_for_fit,
                                      ZucJPose* blend_jpos_for_fit,
                                      KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                                      KINEMATICS_INVERSE_FLAGS* toppra_iflags,
                                      double tmag_abc);                                                      //根据运动指令得到离散点
extern int get_movc_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit);  //根据运动指令得到离散点
extern int get_movc_planning_params(MovcStruct* circle, ZucPose const* const start, ZucPose const* const mid, ZucPose const* const end);
extern int get_movc_pos(MovcStruct* circle, const double portion, ZucPose* discrete_cartpos);
extern int quaternion_interp(PmRotationMatrix* const Rot_0, PmRotationMatrix* const Rot_1, const double t, ZucPose* discrete_cartpos);
extern void rot2quat(PmRotationMatrix* const m, double* q);
extern int set_constraints_toppra(ConstraintsToppra& constraints);                                //设置toppra约束：关节约束、末端线速度约束
extern int get_path_information(const int jpos_len, ConstraintsToppra& constraints);              //获得用于求解最优化的系数
extern int get_blend_path_information(const int blend_jpos_len, ConstraintsToppra& constraints);  //获得用于求解最优化的系数
extern int ClearBlendQueue();
extern int BlendEnqueue(BlendJpos element_added);
extern int GetBlendFront(BlendJpos* element_front);
extern int joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
extern int joint_acc_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
extern int joint_tor_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
extern int blend_joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
extern int blend_joint_acc_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
extern int blend_joint_tor_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
extern int interpolate_discretization(const int jpos_len);
extern int blend_interpolate_discretization(const int blend_jpos_len);
extern int compute_pathvel_by_reqvel(const int jpos_len);
extern double get_min(double a, double b);
extern double get_max(double a, double b);
extern void init_DYN();
extern int get_unit_vector(double* vector, double mag, double* unit_vector);
extern int get_line_position(ZucPose* start, double* dist_unit_vector, const double len, ZucPose* line_position);
extern int get_line_gesture(ZucPose* start, ZucPose* end, const double len, ZucPose* gesture);
extern int rpy_quat_convert(double r, double p, double y, double* quat);
extern void quartic_basic(const double t, double* B);  //四元数基函数
extern int get_quat_inv(double* quat, double* quat_inv);
extern int quat_quat_mult(double* quat1, double* quat2, double* quat_out);
extern void power_quat(const double* q, const double t, double* Q);  //四元数乘方运算
extern void log_quat(const double* q, double* Q);                    //四元数对数运算
extern void exp_quat(const double* q, double* Q);                    //四元数指数运算
extern void norm_quat(const double* q, double* q_n);                 //四元数模长
extern int quat_rpy_convert(double* quat, PmRpy* const rpy);

extern int zucMotionKineInverse(int robid, const struct ZucPose* world, double* joint);

/*interfaces for servo move filters*/

extern int zucMotionKineForward(int robid, const double* joint);
extern int zucAbortCleanup(int reason, const char* message = "");
extern int zucServoUpGrade(void);
extern int zucSetCapture(int captId, IOTrigger trigger, int stateToCapt);
extern int zucClearCapture(int captId);
extern int zucSafetyCrc(unsigned int safetycrc);

/*interfaces only applies to CAB 2.1*/
extern int zucShutDown(void);

extern int zucHalt();
extern int zucAbort();

int zucSetMaxFeedOverride(double maxFeedScale);
int zucSetupArcBlends(int arcBlendEnable,
                      int arcBlendFallbackEnable,
                      int arcBlendOptDepth,
                      int arcBlendGapCycles,
                      double arcBlendRampFreq,
                      double arcBlendTangentKinkRatio);

extern int zucClearError(int robot_id);

extern int zucSetBaseOffset(int robot_id, const double* base_offset);

extern int zucSetSimRobot(int robot_id, int sim_robot);